/*
 *  PhysicsEngine.cpp
 *  GLRender
 *
 *  Created by Patrick zulian on 5/4/10.
 *  Copyright 2010 University  of Lugano. All rights reserved.
 *
 */

#include "Physics.h"


Physics * Physics::INSTANCE = NULL;

Physics * Physics::getInstance()
{
	if (INSTANCE == NULL) {
		INSTANCE = new Physics();
	}	
	return INSTANCE;
}

void 
Physics::rotateObject(WMovingObject * object, const double alpha, const double beta, const double gamma)
{
	
	
	if (alpha != 0) {
		Point3d velocity = object->velocity();
		Point3d upperNormal = object->upperNormal();
		
		double norm = velocity.norm();
	
		Point3d sideNormal = (velocity^upperNormal);
		Matrix4d R = Matrix4d::rotation(alpha, sideNormal); 
		
		velocity = (R*velocity);
		velocity.normalize();
		velocity*=norm;
		
		upperNormal = R*upperNormal;
		upperNormal.normalize();
		
		object->velocity() = velocity;
		object->upperNormal() = upperNormal;

		
	}
	
	if (beta != 0) {
		Point3d velocity = object->velocity();
		Point3d upperNormal = object->upperNormal();
		
		double norm = velocity.norm();
		
		Matrix4d R = Matrix4d::rotation(beta, velocity); 
		
		velocity = (R*velocity);
		velocity.normalize();
		velocity*=norm;
		
		upperNormal = R*upperNormal;
		upperNormal.normalize();
		
		object->velocity() = velocity;
		object->upperNormal() = upperNormal;
		
	}
	
	if (gamma != 0) {
		Point3d velocity = object->velocity();
		Point3d upperNormal = object->upperNormal();
		
		double norm = velocity.norm();
		
		Matrix4d R = Matrix4d::rotation(gamma, upperNormal); 
		
		velocity = (R*velocity);
		velocity.normalize();
		velocity*=norm;
		
		upperNormal = R*upperNormal;
		upperNormal.normalize();
		
		object->velocity() = velocity;
		object->upperNormal() = upperNormal;
		
	}
}


void 
Physics::collide(WMovingObject * obj1, WObject * obj2)
{
	assert(obj1->mass() != 0.0);
	
	//
	Point3d n = obj2->position() - obj1->position();
	n.normalize();
	
	const double e = 1.0;
	const double m1 = obj1->mass(), m2 = obj2->mass();
	const double c = ( obj1->velocity() - obj2 ->velocity() ) * n;
	obj1->nextVelocity() =  obj1->velocity() - ( (( m2 * c ) / ( m1 + m2 )) * ( 1.0 + e ) * n );
	obj1->upperNormal() = n;
	
	Point3d p = obj1->nextVelocity(); p.normalize();
	//printf("SCALAR PRODUCT %g\n", obj1->upperNormal() * p  );
	
	obj1->position() += p * obj2->getRadius() * 0.5;
}











